#20250108 at1819--20250109at0932 -7.5.1巡检机器人节点
from geometry_msgs.msg import PoseStamped,Pose
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
import rclpy
from rclpy.duration import Duration
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer #坐标监听
from tf_transformations import euler_from_quaternion, quaternion_from_euler #四元数转欧拉角函数
import math #角度转弧度函数

from autopatrol_interfaces.srv import SpeechText #从功能包导入服务接口

from sensor_msgs.msg import Image #消息接口
from cv_bridge import CvBridge #转换图像格式
import cv2 #保存图像




class PartolNode(BasicNavigator):
    def __init__(self, node_name='patrol_node'):
        super().__init__(node_name)

        #声明参数
        self.declare_parameter('initial_point', [0.0, 0.0, 0.0])
        self.declare_parameter('target_points', [0.0, 0.0, 0.0, 1.0, 1.0, 1.57])
        self.declare_parameter('image_save_path', '')
        self.initial_point_ = self.get_parameter('initial_point').value
        self.target_points_ = self.get_parameter('target_points').value
        self.image_save_path = self.get_parameter('image_save_path').value
        #相机功能：，将订阅到的消息转换成OpenCV格式保存
        self.cv_bridge_ = CvBridge()
        self.latest_img_ = None
        self.img_sub_ = self.create_subscription(Image,  'camera_sensor/image_raw', self.img_callback, 1)
        
        #实时获取TF 相关定义
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)

        #合成语音客户端：调用服务'speech_text'，第一个参数是接口
        self.speech_client_ = self.create_client(SpeechText,'speech_text') 

        #定义函数
    def img_callback(self,msg):
        """
        图像消息回调函数，
        """
        self.latest_img_ = msg
        
    def record_img(self):#用于将接收到的ROS2图像消息转换为OpenCV格式并按指定路径保存
        if self.latest_img_ is not None:
            pose = self.get_current_pose()
            try:
                # 将ROS2的图像消息转换为OpenCV格式的图像
                cv_image = self.cv_bridge_.imgmsg_to_cv2(self.latest_img_, desired_encoding='bgr8')

                # 使用OpenCV保存图像，按照传入的image_save_path参数指定的路径和文件名保存图像
                cv2.imwrite(f'{self.image_save_path}img_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png', cv_image)
                self.get_logger().info(f'图像已成功保存为{self.image_save_path}')
            except Exception as e:
                self.get_logger().error(f'图像转换或保存出现错误: {str(e)}')
    
    def get_pose_by_xyyaw(self, x, y, yaw):
        """
            return PoseStampted对象
        """
        pose =  PoseStamped()
        pose.header.frame_id = 'map'
        pose.pose.position.x = x
        pose.pose.position.y = y

        #返回顺序是xyzw,
        quat = quaternion_from_euler(0, 0, yaw)
        pose.pose.orientation.x =quat[0]
        pose.pose.orientation.y =quat[1]
        pose.pose.orientation.z =quat[2]
        pose.pose.orientation.w =quat[3]

        return pose



    def init_robot_pose(self):
        """
            初始化机器人的姿态
        """
        self.initial_point_ = self.get_parameter('initial_point').value
        init_pose = self.get_pose_by_xyyaw(self.initial_point_[0],self.initial_point_[1],self.initial_point_[2])
        self.setInitialPose(init_pose)#方法
        self.waitUntilNav2Active() #等待导航可用

    def get_target_points(self):
        """
            通过参数值 获取 目标点的集合
        """
        points =[]
        self.target_points_ = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points_)/3)):
            x = self.target_points_[index*3]
            y = self.target_points_[index*3+1]
            yaw = self.target_points_[index*3+2]
            points.append([x, y, yaw])
            self.get_logger().info(f'获取到目标点{index}->{x}, {y}, {yaw}')
        return points
        
    def nav_to_pose(self, target_pioint):
        """
            导航到目标点
        """
        #发送目标位置，接受反馈：
        self.goToPose(target_pioint)
        #self.get_logger().info(f'剩余距离：{feedback.distance_remaining}')
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            self.get_logger().info(f'剩余距离：{feedback.distance_remaining}')
            self.get_logger().info(
                f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达'
        )
        # 最终结果判断
        result = self.getResult()
        if result == TaskResult.SUCCEEDED:
            self.get_logger().info('导航结果：成功')
        elif result == TaskResult.CANCELED:
            self.get_logger().warn('导航结果：被取消')
        elif result == TaskResult.FAILED:
            self.get_logger().error('导航结果：失败')
        else:
            self.get_logger().error('导航结果：返回状态无效')


    def get_current_pose(self):
        """
            获取机器人
            当前位姿
        """
        while rclpy.ok():#如果获取失败，尝试再获取
            try:
                tf = self.buffer.lookup_transform(
                    'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
                transform = tf.transform
                rotation_euler = euler_from_quaternion([
                    transform.rotation.x,
                    transform.rotation.y,
                    transform.rotation.z,
                    transform.rotation.w
                ])
                self.get_logger().info(
                    f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}')
                return transform #后面进行朗读
            except Exception as e:
                self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')
    
    def speech_text(self, text):
        """
            调用服务/'speech_text',合成语音
        """
        while not self.speech_client_.wait_for_service(timeout_sec=1):
            self.get_logger().info(f'语音合成服务未上线，等待中...')
        
        request = SpeechText.Request()
        request.text = text 
        future = self.speech_client_.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        if future.result() is not None:
            response = future.result()
            if response.result == True:
                self.get_logger().info(f'语音合成成功{text}')
            else:
                self.get_logger().warn(f'语音合成失败{text}')
        else:
            self.get_logger().warn(f'语音合成响应')



#main函数
def main():
    rclpy.init()
    patrol = PartolNode() #节点-继承自BasicNavigator
    patrol.speech_text('正初始化位置')

    #rclpy.spin(patrol)#为了导出yaml文件：
    patrol.init_robot_pose() #初始化位置
    patrol.speech_text('初始化完成')

    while rclpy.ok():
        points = patrol.get_target_points()
        for point in points:
            x, y, yaw = point[0], point[1], point[2]
            target_pose = patrol.get_pose_by_xyyaw(x, y, yaw)
            patrol.speech_text(f'正准备前往{x},{y} 目标点')
            patrol.nav_to_pose(target_pose )
            patrol.speech_text(f'已经到达目标点{x},{y} 正在准备记录图像')
            patrol.record_img()
            patrol.speech_text(f'记录图像完成')
    
    #BasicNavigator 用到很多spin，所以不用下面2行
    #
    rclpy.shutdown()

if __name__ == '__main__':
    main()
